#pragma once
/// This file is the release version of L-BFGS 2.3 on Nov.25 2022
#include <Eigen/Eigen>
#include <cmath>
#include <algorithm>

namespace math_utils{
    class lbfgs {
        // ----------------------- Data Type Part -----------------------
    public:
        /**
         * L-BFGS optimization parameters.
         */
        struct lbfgs_parameter_t {
            /**
             * The number of corrections to approximate the inverse hessian matrix.
             *  The L-BFGS routine stores the computation results of previous m
             *  iterations to approximate the inverse hessian matrix of the current
             *  iteration. This parameter controls the size of the limited memories
             *  (corrections). The default value is 8. Values less than 3 are
             *  not recommended. Large values will result in excessive computing time.
             */
            int mem_size = 8;

            /**
             * Epsilon for grad convergence test. DO NOT USE IT in nonsmooth cases!
             *  Set it to 0.0 and use past-delta-based test for nonsmooth functions.
             *  This parameter determines the accuracy with which the solution is to
             *  be found. A minimization terminates when
             *      ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon,
             *  where ||.||_inf is the infinity norm. The default value is 1.0e-5.
             *  This should be greater than 1.0e-6 in practice because L-BFGS does
             *  not directly reduce first-order residual. It still needs the function
             *  value which can be corrupted by machine_prec when ||g|| is small.
             */
            double g_epsilon = 1.0e-5;

            /**
             * Distance for delta-based convergence test.
             *  This parameter determines the distance, in iterations, to compute
             *  the rate of decrease of the cost function. If the value of this
             *  parameter is zero, the library does not perform the delta-based
             *  convergence test. The default value is 3.
             */
            int past = 3;

            /**
             * Delta for convergence test.
             *  This parameter determines the minimum rate of decrease of the
             *  cost function. The library stops iterations when the following
             *  condition is met:
             *      |f' - f| / max(1, |f|) < delta,
             *  where f' is the cost value of past iterations ago, and f is
             *  the cost value of the current iteration.
             *  The default value is 1.0e-6.
             */
            double delta = 1.0e-6;

            /**
             * The maximum number of iterations.
             *  The lbfgs_optimize() function terminates an minimization process with
             *  ::LBFGSERR_MAXIMUMITERATION status code when the iteration count
             *  exceedes this parameter. Setting this parameter to zero continues an
             *  minimization process until a convergence or error. The default value
             *  is 0.
             */
            int max_iterations = 0;

            /**
             * The maximum number of trials for the line search.
             *  This parameter controls the number of function and gradients evaluations
             *  per iteration for the line search routine. The default value is 64.
             */
            int max_linesearch = 64;

            /**
             * The minimum step of the line search routine.
             *  The default value is 1.0e-20. This value need not be modified unless
             *  the exponents are too large for the machine being used, or unless the
             *  problem is extremely badly scaled (in which case the exponents should
             *  be increased).
             */
            double min_step = 1.0e-20;

            /**
             * The maximum step of the line search.
             *  The default value is 1.0e+20. This value need not be modified unless
             *  the exponents are too large for the machine being used, or unless the
             *  problem is extremely badly scaled (in which case the exponents should
             *  be increased).
             */
            double max_step = 1.0e+20;

            /**
             * A parameter to control the accuracy of the line search routine.
             *  The default value is 1.0e-4. This parameter should be greater
             *  than zero and smaller than 1.0.
             */
            double f_dec_coeff = 1.0e-4;

            /**
             * A parameter to control the accuracy of the line search routine.
             *  The default value is 0.9. If the function and gradient
             *  evaluations are inexpensive with respect to the cost of the
             *  iteration (which is sometimes the case when solving very large
             *  problems) it may be advantageous to set this parameter to a small
             *  value. A typical small value is 0.1. This parameter should be
             *  greater than the f_dec_coeff parameter and smaller than 1.0.
             */
            double s_curv_coeff = 0.9;

            /**
             * A parameter to ensure the global convergence for nonconvex functions.
             *  The default value is 1.0e-6. The parameter performs the so called
             *  cautious update for L-BFGS, especially when the convergence is
             *  not sufficient. The parameter must be positive but might as well
             *  be less than 1.0e-3 in practice.
             */
            double cautious_factor = 1.0e-6;

            /**
             * The machine precision for floating-point values. The default is 1.0e-16.
             *  This parameter must be a positive value set by a client program to
             *  estimate the machine precision.
             */
            double machine_prec = 1.0e-16;
        };

        /**
         * Return values of lbfgs_optimize().
         *  Roughly speaking, a negative value indicates an error.
         */
        enum {
            /** L-BFGS reaches convergence. */
            LBFGS_CONVERGENCE = 0,
            /** L-BFGS satisfies stopping criteria. */
            LBFGS_STOP,
            /** The iteration has been canceled by the monitor callback. */
            LBFGS_CANCELED,

            /** Unknown error. */
            LBFGSERR_UNKNOWNERROR = -1024,
            /** Invalid number of variables specified. */
            LBFGSERR_INVALID_N,
            /** Invalid parameter lbfgs_parameter_t::mem_size specified. */
            LBFGSERR_INVALID_MEMSIZE,
            /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */
            LBFGSERR_INVALID_GEPSILON,
            /** Invalid parameter lbfgs_parameter_t::past specified. */
            LBFGSERR_INVALID_TESTPERIOD,
            /** Invalid parameter lbfgs_parameter_t::delta specified. */
            LBFGSERR_INVALID_DELTA,
            /** Invalid parameter lbfgs_parameter_t::min_step specified. */
            LBFGSERR_INVALID_MINSTEP,
            /** Invalid parameter lbfgs_parameter_t::max_step specified. */
            LBFGSERR_INVALID_MAXSTEP,
            /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */
            LBFGSERR_INVALID_FDECCOEFF,
            /** Invalid parameter lbfgs_parameter_t::s_curv_coeff specified. */
            LBFGSERR_INVALID_SCURVCOEFF,
            /** Invalid parameter lbfgs_parameter_t::machine_prec specified. */
            LBFGSERR_INVALID_MACHINEPREC,
            /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */
            LBFGSERR_INVALID_MAXLINESEARCH,
            /** The function value became NaN or Inf. */
            LBFGSERR_INVALID_FUNCVAL,
            /** The line-search step became smaller than lbfgs_parameter_t::min_step. */
            LBFGSERR_MINIMUMSTEP,
            /** The line-search step became larger than lbfgs_parameter_t::max_step. */
            LBFGSERR_MAXIMUMSTEP,
            /** Line search reaches the maximum, assumptions not satisfied or precision not achievable.*/
            LBFGSERR_MAXIMUMLINESEARCH,
            /** The algorithm routine reaches the maximum number of iterations. */
            LBFGSERR_MAXIMUMITERATION,
            /** Relative search interval width is at least lbfgs_parameter_t::machine_prec. */
            LBFGSERR_WIDTHTOOSMALL,
            /** A logic error (negative line-search step) occurred. */
            LBFGSERR_INVALIDPARAMETERS,
            /** The current search direction increases the cost function value. */
            LBFGSERR_INCREASEGRADIENT,
        };

        /**
         * Callback interface to provide cost function and gradient evaluations.
         *
         *  The lbfgs_optimize() function call this function to obtain the values of cost
         *  function and its gradients when needed. A client program must implement
         *  this function to evaluate the values of the cost function and its
         *  gradients, given current values of variables.
         *
         *  @param  instance    The user data sent for lbfgs_optimize() function by the client.
         *  @param  x           The current values of variables.
         *  @param  g           The gradient vector. The callback function must compute
         *                      the gradient values for the current variables.
         *  @retval double      The value of the cost function for the current variables.
         */
        typedef double (*lbfgs_evaluate_t)(void *instance,
                                           const Eigen::VectorXd &x,
                                           Eigen::VectorXd &g);

        /**
         * Callback interface to provide an upper bound at the beginning of the current line search.
         *
         *  The lbfgs_optimize() function call this function to obtain the values of the
         *  upperbound of the stepsize to search in, provided with the beginning values of
         *  variables before the line search, and the current step vector (can be descent direction).
         *  A client program can implement this function for more efficient linesearch. Any step
         *  larger than this bound should not be considered. For example, it has a very large or even
         *  inf function value. Note that the function value at the provided bound should be FINITE!
         *  If it is not used, just set it nullptr.
         *
         *  @param  instance    The user data sent for lbfgs_optimize() function by the client.
         *  @param  xp          The values of variables before current line search.
         *  @param  d           The step vector. It can be the descent direction.
         *  @retval double      The upperboud of the step in current line search routine,
         *                      such that (stpbound * d) is the maximum reasonable step.
         */
        typedef double (*lbfgs_stepbound_t)(void *instance,
                                            const Eigen::VectorXd &xp,
                                            const Eigen::VectorXd &d);

        /**
         * Callback interface to monitor the progress of the minimization process.
         *
         *  The lbfgs_optimize() function call this function for each iteration. Implementing
         *  this function, a client program can store or display the current progress
         *  of the minimization process. If it is not used, just set it nullptr.
         *
         *  @param  instance    The user data sent for lbfgs_optimize() function by the client.
         *  @param  x           The current values of variables.
         *  @param  g           The current gradient values of variables.
         *  @param  fx          The current value of the cost function.
         *  @param  step        The line-search step used for this iteration.
         *  @param  k           The iteration count.
         *  @param  ls          The number of evaluations called for this iteration.
         *  @retval int         Zero to continue the minimization process. Returning a
         *                      non-zero value will cancel the minimization process.
         */
        typedef int (*lbfgs_progress_t)(void *instance,
                                        const Eigen::VectorXd &x,
                                        const Eigen::VectorXd &g,
                                        const double fx,
                                        const double step,
                                        const int k,
                                        const int ls);

        /**
         * Callback data struct
         */
        struct callback_data_t {
            void *instance = nullptr;
            lbfgs_evaluate_t proc_evaluate = nullptr;
            lbfgs_stepbound_t proc_stepbound = nullptr;
            lbfgs_progress_t proc_progress = nullptr;
        };

        // ----------------------- L-BFGS Part -----------------------

        /**
         * Line search method for smooth or nonsmooth functions.
         *  This function performs line search to find a point that satisfy
         *  both the Armijo condition and the weak Wolfe condition. It is
         *  as robust as the backtracking line search but further applies
         *  to continuous and piecewise smooth functions where the strong
         *  Wolfe condition usually does not hold.
         *
         *  @see
         *      Adrian S. Lewis and Michael L. Overton. Nonsmooth optimization
         *      via quasi-Newton methods. Mathematical Programming, Vol 141,
         *      No 1, pp. 135-163, 2013.
         */
        static int line_search_lewisoverton(Eigen::VectorXd &x,
                                            double &f,
                                            Eigen::VectorXd &g,
                                            double &stp,
                                            const Eigen::VectorXd &s,
                                            const Eigen::VectorXd &xp,
                                            const Eigen::VectorXd &gp,
                                            const double stpmin,
                                            const double stpmax,
                                            const callback_data_t &cd,
                                            const lbfgs_parameter_t &param);

        /**
         * Start a L-BFGS optimization.
         * Assumptions: 1. f(x) is either C2 or C0 but piecewise C2;
         *              2. f(x) is lower bounded;
         *              3. f(x) has bounded level sets;
         *              4. g(x) is either the gradient or subgradient;
         *              5. The gradient exists at the initial guess x0.
         * A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation
         * callback) and pass the pointer to the callback function to lbfgs_optimize()
         * arguments. Similarly, a user can implement a function compatible with
         * ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and
         * ::lbfgs_progress_t (progress callback) to obtain the current progress
         * (e.g., variables, function, and gradient, etc) and to cancel the iteration
         * process if necessary. Implementation of the stepbound and the progress callback
         * is optional: a user can pass nullptr if progress notification is not necessary.
         *
         *
         *  @param  x               The vector of decision variables.
         *                          THE INITIAL GUESS x0 SHOULD BE SET BEFORE THE CALL!
         *                          A client program can receive decision variables
         *                          through this vector, at which the cost and its
         *                          gradient are queried during minimization.
         *  @param  f               The ref to the variable that receives the final
         *                          value of the cost function for the variables.
         *  @param  proc_evaluate   The callback function to provide function f(x) and
         *                          gradient g(x) evaluations given a current values of
         *                          variables x. A client program must implement a
         *                          callback function compatible with lbfgs_evaluate_t
         *                          and pass the pointer to the callback function.
         *  @param  proc_stepbound  The callback function to provide values of the
         *                          upperbound of the stepsize to search in, provided
         *                          with the beginning values of variables before the
         *                          line search, and the current step vector (can be
         *                          negative gradient). A client program can implement
         *                          this function for more efficient linesearch. If it is
         *                          not used, just set it nullptr.
         *  @param  proc_progress   The callback function to receive the progress
         *                          (the number of iterations, the current value of
         *                          the cost function) of the minimization
         *                          process. This argument can be set to nullptr if
         *                          a progress report is unnecessary.
         *  @param  instance        A user data pointer for client programs. The callback
         *                          functions will receive the value of this argument.
         *  @param  param           The parameters for L-BFGS optimization.
         *  @retval int             The status code. This function returns a nonnegative
         *                          integer if the minimization process terminates without
         *                          an error. A negative integer indicates an error.
         */
        static int lbfgs_optimize(Eigen::VectorXd &x,
                                  double &f,
                                  lbfgs_evaluate_t proc_evaluate,
                                  lbfgs_stepbound_t proc_stepbound,
                                  lbfgs_progress_t proc_progress,
                                  void *instance,
                                  const lbfgs_parameter_t &param);

        /**
         * Get string description of an lbfgs_optimize() return code.
         *
         *  @param err          A value returned by lbfgs_optimize().
         */
        static const char *lbfgs_strerror(const int err);

    }; // namespace lbfgs

}
